/*
 * Team D closed-loop arm control code.
 * @author Derek Leung
 */


//=====External=====//

/* Specification */

const float P_DB = 1;
const int DB_TOLERANCE = 1;

//-----drive base functions-----//

const double center_wheel = 2.85;
//calibration to be done at initialization
int drive_base_offset_left;
int drive_base_offset_right;

//toggle ultrasonic
bool use_ultra = true;

//targets
int drive_base_target_linear = 0; //inches
int drive_base_target_angular = 0; //degrees

//returns true when target is reached
bool drive_base_target_reached = true;

//flag for whether to actively reach target
bool drive_base_move = false;

//notifies at distance reached, -1 for inactive or reached
int drive_base_alert = -1;

//control loop to reach target when drive_base_move is true
task regulate_drive_base();

//=====Internal=====//
int ultra_start = 0;
int dist_covered = 0;
int lmod;
int rmod;
//int togo;
task regulate_drive_base()
{

  drive_base_target_reached = false;
  //read sensor value of ultrasound if init
  ultra_start = SensorValue(ultra);

  while(drive_base_move)
  {
    if(use_ultra)
      dist_covered = ultra_start - SensorValue(ultra);
    else
      dist_covered = SensorValue(lshaft) - drive_base_offset_left; //uses left drive base error
    drive_base_target_reached = false;
	  if(drive_base_alert != -1) {
	    if(dist_covered > drive_base_alert) {
	      drive_base_alert = -1;
	    }
	  }
    if(dist_covered >= drive_base_target_linear) //TODO include angular
    {
	    //TODO refine motor kill
	    motor[leftDBMotor] = 0;
	    motor[rightDBMotor]= 0;
	    drive_base_target_reached = true;
      drive_base_move = false; //TODO does not include angular movement
    } else
    {
//      togo = drive_base_target_linear - ultra_value;
/*      lmod = drive_base_target_linear-((int)(center_wheel*(SensorValue[ultra] - drive_base_offset_left))); //TODO implement turns?
      rmod = drive_base_target_linear-((int)(center_wheel*(SensorValue[rShaft] - drive_base_offset_right)));

      motor[leftDBMotor] = 127-(l_togo-r_togo);
      motor[rightDBMotor]= 127-(r_togo-l_togo);*/
      //P loop
      motor[leftDBMotor] = P_DB * (dist_covered - drive_base_target_linear);
      motor[rightDBMotor]= P_DB * (dist_covered - drive_base_target_linear);
      //Adjust for non-straightness
//      int error = dist_covered - (SensorValue(rshaft) - drive_base_offset_right) * -1;
//      motor[rightDBMotor]+= error;
    } /*
    if(drive_base_target_angular != 0)
    {

      //Implement angular correction

    } */
  }
}
